import numpy as np

from solder_splash import *


def get_real_bottom_tin_contour(x_points, y_points, z_points, std_npy, start_point):
    bottom_down, bottom_up, bottom_left = get_bottom_value(x_points, y_points, z_points)
    Z_BOTTOM = (bottom_down + bottom_up + bottom_left) / 3  # PCD面的大致底面高度
    x_points, y_points, z_points = data_filter_by_area(x_points, y_points, z_points, x_min=start_point[1] / RADIO,
                                                       x_max=(start_point[1] + std_npy.shape[1]) / RADIO,
                                                       y_min=start_point[0] / RADIO,
                                                       y_max=(start_point[0] + std_npy.shape[0]) / RADIO)
    real_bottom = get_real_bottom(Z_BOTTOM, z_points)
    return real_bottom


def show_3d_add_bottom_tin_contour(npy, window_name="3d"):
    """
    3d显示
    Args:
        npy:
        window_name:

    Returns:

    """
    xyz_points = parse_npy_(npy)  # 得到x，y，z坐标
    x_points, y_points, z_points = xyz_points
    bottom = np.zeros(npy.shape)
    xyz_zero = parse_npy_(bottom)
    x_z, y_z, z_z = xyz_zero
    x_points = np.hstack((x_points, x_z))
    y_points = np.hstack((y_points, y_z))
    z_points = np.hstack((z_points, z_z))
    o3d.visualization.draw_geometries([numpy2pcd(x_points, y_points, z_points)], window_name=window_name)


def draw_binary_pic_tin_contour(npy, target):
    pic = np.ones(npy.shape)
    min_val = npy.min()
    max_val = npy.max()
    pic = (npy - min_val) * 255 / (max_val - min_val)
    pic = pic.astype(np.uint8)
    # pic = cv2.imread(
    #     "D:\\Programs\\data\\dataset\\20220701-0628-full-size\\2022-06-28\\3505277134ML085B085301110012200465_up_2022-06-28-09-04-52_meta.png")
    # pic = cv2.cvtColor(pic, cv2.COLOR_BGR2GRAY);
    ret, pic = cv2.threshold(pic, 100, 255, cv2.THRESH_BINARY);
    # zero_index = npy <= bottom
    # pic[zero_index] = 0
    # cv2.imshow("test", pic)
    # cv2.waitKey(0)
    cv2.imwrite(target, pic)
    find_contour_tin_contour(pic)


def find_contour_tin_contour(gray):
    contours, hierarchy = cv2.findContours(gray, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
    print(len(contours))
    x = 0
    for i in range(len(contours)):
        area = cv2.contourArea(contours[i])
        if area > 2000:
            print(area)
            x = i
    cnt = contours[x]
    img = gray.copy()
    approx1 = cv2.approxPolyDP(cnt, 3, True)  # 拟合精确度
    img = cv2.polylines(img, [approx1], True, (255, 255, 0), 2)
    cv2.namedWindow("approxPolyDP", cv2.WINDOW_NORMAL)
    cv2.imshow('approxPolyDP', img)

    # cv2.imwrite("dst.png", img)
    print(len(approx1))
    cv2.waitKey(0)
    cv2.destroyAllWindows()


if __name__ == "__main__":
    std_4_3 = np.load(STD_4_3)
    xyz_points = parse_npy_(std_4_3)  # 得到x，y，z坐标
    x_points, y_points, z_points = xyz_points
    bottom43 = get_real_bottom_tin_contour(x_points, y_points, z_points, std_4_3, (0, 0))
    draw_binary_pic_tin_contour(std_4_3, os.path.join("D:\\Programs\\data\\exp\\binary_contour", "4-3.png"))
    std_4_3 = std_4_3 - bottom43 - 0.1
    show_3d_add_bottom_tin_contour(std_4_3)

    std_4_4 = np.load(STD_4_4)
    xyz_points = parse_npy_(std_4_4)  # 得到x，y，z坐标
    x_points, y_points, z_points = xyz_points
    bottom44 = get_real_bottom_tin_contour(x_points, y_points, z_points, std_4_4, (0, 0))
    draw_binary_pic_tin_contour(std_4_4, os.path.join("D:\\Programs\\data\\exp\\binary_contour", "4-4.png"))
    std_4_4 = std_4_4 - bottom44 - 0.1
    show_3d_add_bottom_tin_contour(std_4_4)

    std_4_5 = np.load(STD_4_5)
    xyz_points = parse_npy_(std_4_5)  # 得到x，y，z坐标
    x_points, y_points, z_points = xyz_points
    bottom45 = get_real_bottom_tin_contour(x_points, y_points, z_points, std_4_5, (0, 0))
    draw_binary_pic_tin_contour(std_4_5, os.path.join("D:\\Programs\\data\\exp\\binary_contour", "4-5.png"))
    std_4_5 = std_4_5 - bottom44 - 0.1
    show_3d_add_bottom_tin_contour(std_4_5)
